# -*- coding:utf-8 -*-
from ctypes import *
import time
import numpy as np

# 导入自定义函数和CANFD SDK动态链接库>>>
from os.path import dirname, join
import sys
import platform
arch = platform.machine()
sys.path.append(join(dirname(__file__), 'src/lysdemo_'+arch))
sys.path.append(join(dirname(__file__), "src"))
sys.path.append(dirname(__file__))
from src import *

# ###########################################################################


if __name__ == "__main__":
    # >>>>>初始化CAN卡部分>>>
    # 设置can or canfd接口
    lcanlib = LCAN()
    # 清除通信缓存区
    ret = lcanlib.LCAN_ClearBuffer(LCAN_USBCANFDMini, devIndex, canIndex)

    # open device
    ret = lcanlib.LCAN_OpenDevice(LCAN_USBCANFDMini, devIndex)

    time.sleep(0.1)
    if ret != LCAN_STATUS_OK:
        print("LYS USBCANFDMini Open Device failed!")
        exit(0)
    # open success
    print("LYS USBCANFDMini Open Device Success!")

    

    # 清除通信缓存区
    ret = lcanlib.LCAN_ClearBuffer(LCAN_USBCANFDMini, devIndex, canIndex)

    # 初始化canfd
    ret = lcanlib.LCAN_InitCANFD(LCAN_USBCANFDMini, devIndex, canIndex, canfd_init_cfg)

    time.sleep(0.1)
    if ret != LCAN_STATUS_OK:
        print("LYS USBCANmini Init Failed!")
    else:
        print("LYS USBCANmini Init Success!")
    

    # 清除can通信缓存区
    ret = lcanlib.LCAN_ClearBuffer(LCAN_USBCANFDMini, devIndex, canIndex)
    print('sleeping')

    # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
    # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

    motor_controllers = MotorController(motor_num)
    
    motor_controllers = MotorController(motor_num)

    # 清除错误
    ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFDMini, devIndex, canIndex, motor_controllers.can_clear_err, motor_num)
    print('sleeping')
    time.sleep(0.1)

    # 上使能
    ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFDMini, devIndex, canIndex, motor_controllers.can_enable, motor_num)
    print('sleeping')
    time.sleep(0.1)
    if ret != LCAN_STATUS_OK:
        print("Enable Failed!")
    else:
        print("Enable Success!")
    
    


    # 在线更新标志位
    ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFDMini, devIndex, canIndex, motor_controllers.can_obj_init, motor_num)
    print('sleeping')
    time.sleep(0.1)
    if ret != LCAN_STATUS_OK:
        print("motor init Failed!")
    else:
        print("motor init Success!")



    # 设置c模式
    ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFDMini, devIndex, canIndex, motor_controllers.can_c_mode, motor_num)
    print('sleeping')
    time.sleep(0.1)
    if ret != LCAN_STATUS_OK:
        print("Set c mode failed!")
    else:
        print("Set c mode success!")

    # # 设置v模式
    # ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFDMini, devIndex, canIndex, motor_controllers.can_v_mode, motor_num)
    # print('sleeping')
    # time.sleep(0.1)
    # if ret != LCAN_STATUS_OK:
    #     print("Set v mode failed!")
    # else:
    #     print("Set v mode success!")




    # >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
    # 控制
    # 清除通信缓存区
    ret = lcanlib.LCAN_ClearBuffer(LCAN_USBCANFDMini, devIndex, canIndex)
    time.sleep(0.1)
    # 发送与读取>>>
    # v Mode

    start = time.time()
    damp = 1e-20
    intv = 500
    intp = 20000
    intc = 2000
    control_time = 1
    motor_controllers.set_p(np.array([0, 0, 0, 0, 0, intp])+damp)
    motor_controllers.set_v(np.array([0, 0, 0, 0, 0, intv])+damp)
    motor_controllers.set_c(np.array([0, 0, 0, 0, 0, intc])+damp)
    
    time.sleep(5)
    start = time.time()
    while time.time() - start < control_time:
        # ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFDMini, devIndex, canIndex, motor_controllers.can_set_c, motor_num)
        ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFDMini, devIndex, canIndex, motor_controllers.can_set_c[5], 1)
        # ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFDMini, devIndex, canIndex, motor_controllers.can_set_v[5], 1)
        time.sleep(0.01)

        
                
    motor_controllers.set_p(np.array([0, 0, 0, 0, 0, -intp])+damp)
    motor_controllers.set_v(np.array([0, 0, 0, 0, 0, -intv])+damp)
    motor_controllers.set_c(np.array([0, 0, 0, 0, 0, -intc])+damp)
    start = time.time()
    while time.time() - start < control_time:
        # ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFDMini, devIndex, canIndex, motor_controllers.can_set_c, motor_num)
        ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFDMini, devIndex, canIndex, motor_controllers.can_set_c[5], 1)
        # ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFDMini, devIndex, canIndex, motor_controllers.can_set_v[5], 1)
        time.sleep(0.1)

        # 读取状态
        new_pvc = receive_pvc(lcanlib, devIndex, canIndex)
        if new_pvc is not None:
            pvc = new_pvc  # 仅当有有效数据时更新
            print(f"最新状态:+ {pvc}")
            # print(f"time cost: {loop_time}")
        else:
            print(f"get no pvc data\n")
    
    time.sleep(control_time)
   
    motor_controllers.set_p(np.array([0, 0, 0, 0, 0, 0])+damp)
    motor_controllers.set_v(np.array([0, 0, 0, 0, 0, 0])+damp)
    motor_controllers.set_c(np.array([0, 0, 0, 0, 0, 0])+damp)

    # ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFDMini, devIndex, canIndex, motor_controllers.can_set_c, motor_num)
    ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFDMini, devIndex, canIndex, motor_controllers.can_set_c[5], 1)
    # ret = lcanlib.LCAN_TransmitFD(LCAN_USBCANFDMini, devIndex, canIndex, motor_controllers.can_set_v[5], 1)
    time.sleep(2*control_time)

    # 发送与读取<<<

    # 清除通信缓存区
    ret = lcanlib.LCAN_ClearBuffer(LCAN_USBCANFDMini, devIndex, canIndex)
    # <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
    time.sleep(0.1)

    #Close Device
    lcanlib.LCAN_CloseDevice(LCAN_USBCANFDMini, devIndex)
    print("LYS USBCANmini Close!")
